#include <MotorControl.h>
#include <ColorSensor.h>
#include <DistanceSensor.h>

#include "Arduino.h"

MotorControl motorControl(3, 2, 5, 4, 7);
ColorSensor frontSensor(8);
ColorSensor backSensor(9);
DistanceSensor distanceSensor(A0);

int buttonPin = 12;
int ledPin =  13;
long startupTime = 5000; // [milli seconds]

boolean started = false;

boolean searchingStarted = false;
boolean searchDriveStarted = false;
long searchingStartedAt = 0;

boolean enemySpotted = false;
boolean enemyStarted = false;
long enemySpottedAt = 0;

boolean whiteSeen = false;
long whiteSeenAt = 0;

boolean backingUp = false;
long backingUpAtTime = 0;

void setup() {
  Serial.begin(9600);
	pinMode(buttonPin, INPUT);
	pinMode(ledPin, OUTPUT);
	digitalWrite(ledPin, LOW); //start with the led off
}

void resetState() {
	searchingStarted = false;
	searchingStartedAt = 0;

	enemySpotted = false;
	enemySpottedAt = 0;

	whiteSeen =false;

	backingUp = false;
	backingUpAtTime = 0;
}

void startupProcedure() {
        for (int i = 0; i < 5; i++) {
                 digitalWrite(ledPin, HIGH);
	         delay(500);
                 digitalWrite(ledPin, LOW);
	         delay(500);
        }
}

void loop() {
	//handles a button press
	if (digitalRead(buttonPin) == HIGH) {
		while (digitalRead(buttonPin) == HIGH) {
                        digitalWrite(ledPin, LOW);
			delay(20);
			//doe niets
		}
		if (! started) {
                        startupProcedure();
			resetState();
			digitalWrite(ledPin, HIGH);
			started = true;
		} else {
                        motorControl.stopMotors(0);
			digitalWrite(ledPin, LOW);
			started = false;
		}
	}

	if (started) {
		//driveSafely();
		Serial.println(distanceSensor.getDistance());

		//enemySpotted = false;

		// check for white line
		if (seesWhite()) {    //TODO als voor en achter wit ziet draaien
                        enemyStarted = false;
                        searchDriveStarted = false;

			motorControl.stopMotors(0);
			motorControl.setDriveSpeed(150);
			if (frontIsWhite()) {
                                if (backIsWhite()) {
                                      motorControl.turn(true);
                                } else {
                                      motorControl.stopMotors(0);
				      motorControl.drive(false, 50);
                                }
			} else {
                                motorControl.stopMotors(0);
				motorControl.drive(true, 50);
                        }

			backingUp = true;
			backingUpAtTime = millis();
		}
		/*
		// check for enemy
		if (seesEnemy()) {
			motorControl.stopMotors(50);
			enemySpotted = true;
			enemySpottedAt = millis();
		}*/

		// backup from white line
		if (backingUp) {
			if (millis() > backingUpAtTime + 1500) {
				motorControl.stopMotors(50);
				backingUp = false;
			}
			else if (millis() > backingUpAtTime + 1000) {
				motorControl.turn(true);
			}
		}
                /*else {
                        motorControl.drive(true);
                }*/
		// charge to enemy
		else if (seesEnemy()) {
                        motorControl.setDriveSpeed(255);
    			motorControl.drive(true);
                        if (! enemyStarted) {
                            enemySpottedAt = millis();
                            enemyStarted = true;
    			    motorControl.setDriveSpeed(255);
    			    motorControl.drive(true);
                        }
                        if (millis() > enemySpottedAt + 12000) {
                            enemyStarted = false;
                            motorControl.stopMotors(0);
                        } else if (millis() > enemySpottedAt + 10000) {
                           motorControl.stopMotors(0);
                           motorControl.drive(false);
                        }
		}
		// search enemy
		else {
                        enemyStarted = false;
			if (! searchingStarted) {
				searchingStarted = true;
                                searchDriveStarted = false;
				searchingStartedAt = millis();
				motorControl.setTurnSpeed(175);
				motorControl.turn(true);
			} else {
				if (millis() > searchingStartedAt + 5500) {
                                        motorControl.stopMotors(0);
					searchingStarted = false;
				} else if (millis() > searchingStartedAt + 2500) {
                                        if (! searchDriveStarted) {
                                                searchDriveStarted = true;
					        motorControl.drive(true);
                                        }
				}
			}
		}
	}
}

void reset() {

}

void driveSafely() {
	motorControl.drive(true);
	while (! seesWhite() ) {
	}
        motorControl.stopMotors(0);
	motorControl.drive(false);
	delay(1500);
	motorControl.turn(true);
	delay(1000);
	motorControl.stopMotors();
}

boolean seesWhite() {
	return backIsWhite() || frontIsWhite();
}

boolean backIsWhite() {
	return backSensor.getValue() == LOW;

}

boolean frontIsWhite() {
	return frontSensor.getValue() == LOW;
}

boolean findEnemy() {
	motorControl.turn(true);
	int distance = distanceSensor.getDistance();
	while(distance > 30) {}
	return true;
}

boolean seesEnemy() {
        int distance = distanceSensor.getDistance();
	return (distance <= 50) && (distance > 0);
}

void calibrateTurnTime() {
	motorControl.drive(true, 170);
	while (frontSensor.getValue() == HIGH) {
	}
	//stopMotors();
	//delay(2000);
	motorControl.stopMotors(10);
	double time = millis();
	motorControl.turn(true);
	while (backSensor.getValue() == HIGH) {
	}
	motorControl.stopMotors(10);
	time = millis() - 10 - time;
	motorControl.setTurnTime(time);
	Serial.println(time);
}
